/*******************************************************************
*
*    FILE: robot_interrupts.c
*    Description: 
*    AUTHOR: Tim Cook
*    DATE:9/20/2009
*
*******************************************************************/

/** include files **/
#include <p18f4520.h>
#include <adc.h>
#include "ieee_robot.h"
#include "servo.h"
#include "motor.h"

void InterruptHandlerHigh ();
void InterruptHandlerLow ();

/** external data **/
extern unsigned short int servo_position;
extern unsigned short int SLOW_MOTOR_FLAG;
extern unsigned short int a2d_result; 
extern unsigned char servo_command[CMD_SIZE];
extern short pos;

/** public functions **/
#pragma code InterruptVectorHigh = 0x08
void InterruptVectorHigh (void) {
    _asm
        goto InterruptHandlerHigh //jump to interrupt routine
     _endasm
}

#pragma code InterruptVectorLow = 0x18
void InterruptVectorLow(void){
	_asm
		goto InterruptHandlerLow
	_endasm
}

#pragma code
#pragma interrupt InterruptHandlerHigh
void InterruptHandlerHigh () {
    if (PIR1bits.TXIF) {
        if (pos < 2) {
            TXREG = servo_command[++pos];
        } else {
            pos = -1;
            PIE1 = PIE1 & (~0x10);
        }
    }
    /*
    if (TMR0_INT_FLAG) {            //check for TMR0 overflow
        INTCONbits.TMR0IF = 0;      //clear interrupt flag for TMR1
        write_timer0( TMR0_VALUE );   //write the value to TMR0
            //required to generate 50 Hz interrrupts that is defined
            //in servo.h

        //write direction to turn and
        //enable TMR1 interrupts
        write_timer1( servo_position );
        PIE1 = PIE1 | 0x01;

        //write to servo
        PORTBbits.RB1 = 1;     //write a 1 to servo port

        //enable TMR1 peripheral
        //T1CON = T1CON | 0x01; 
        
    } else if ( TMR1_INT_FLAG ){
        PIR1 = PIR1 & 0xFE; //clear TMR1 interrupt flag
        PIE1 = PIE1 & 0xFE; //disable TMR1 interrupts
        //T1CON = T1CON & (~0x01); //disable T1

        //clear servo pin for low part of cycle
        PORTBbits.RB1 = 0;
    }*/
    INTCONbits.GIEH = 1; //renable global interrupts
    INTCONbits.PEIE = 1; //renable peripheral interrups
}
//rtfm_bdctfm


#pragma code
#pragma interrupt InterruptHandlerLow
void InterruptHandlerLow(){
//********************ADC INTERRUPT**********************************************
//===============================================================================	
	if (PIR1bits.ADIF == 1){		//ADC caused interrupt
		a2d_result = ReadADC();	//grab conversion
		//PORTB = a2d_result;		//for debugging
		ADCON0bits.ADON = 0;	//turn off GO bit
		PIR1bits.ADIF = 0;		//turn off interrrupt flag
		
		
	//	if( (ADCON0 & 0x3C) ==  0){		//VCC CONVERSION
			if( a2d_result < VCC_THRESH ){ //if vcc dipped lower than threshold
				SetChanADC(ADC_CH1);		//use adc to check photo detector next
				PORTAbits.RA2 = 1;	//send current to photo detector
			}
			else{
				SLOW_MOTOR_FLAG = 0;	//no voltage shortage do not slow motor
			}
	//	}
	/*	else if ( (ADCON0 & 0x3C) ){		//PHOTO DETECTOR CONVERSION
			PORTAbits.RA2 = 0;			//stop sending current to photo detector
			if ( a2d_result < PHOTO_THRESH ) {	//high light point
				SLOW_MOTOR_FLAG = 1;	//slow the motor to charge longer
			}
			else {
				SLOW_MOTOR_FLAG = 0;
			}
			SetChanADC(ADC_CH0);			//go back and re-check vcc
 		}*/

          PIE1bits.ADIE = 1;	//enable ADC interrupts
	    //ADCON0 = ADCON0 | 0x03;		//start next conversion
          return;
	}
//===================================================================================
//*********************************END ADC INTERRUPT*********************************



//===================================================================================
//********************************WHISKER INTERRUPT**********************************
//!!!!Note:  1 external interrupt is used for front whisker.  change when bumper is 
//implemented.  in which case use status change interrupts RB<7:4> for all three whiskers.
//however, status change ports KBI2 and KBI3 (pins 6 and 7)are implemented on same  
//ports as in circuit debugger in cicuit serial programming clock and data pins.  
//therefore, they will not be available for debugging.



//whisker interrupt debug code

	//INTCON3bits.INT2F = 0;  //clear flags
	//INTCON3bits.INT1F = 0; 	
	//INTCONbits.RBIF = 0;
	if((INTCON3bits.INT2F == 1)||(INTCONbits.RBIF == 1)){
		if((PORTB & 0x04)==0){				//front whisker no contact
			if((PORTB & 0x10) == 0) { //RB4 low
				servo_position = SERVO_LEFT_45;
			}
			else servo_position = SERVO_LEFT_23;
		}
		else {							//front whisker contact
			if((PORTB & 0x20) == 0) { //RB5 low
				servo_position = SERVO_RIGHT_23;
			}
			else servo_position = SERVO_STRAIGHT;		
		}	
		INTCON2bits.INTEDG2 = ((~INTCON2bits.INTEDG2) & INTCON3bits.INT2F) | ((~INTCON3bits.INT2F) & INTCON2bits.INTEDG2);
		PORTBbits.RB3 = INTCON3bits.INT2F;			//debug purpose
		PORTBbits.RB0 = INTCONbits.RBIF;
		INTCON3bits.INT2F = 0;  //clear flags
		INTCONbits.RBIF = 0;
		INTCONbits.RBIE = 1;		  //re-enable RB port change interrupts
		INTCON3bits.INT2IE = 1;		  //enable INT2
		//INTCON3bits.INT1F = 0; 
	}

	//INTCON3 = INCON3 | 0x18;    //enable INT1 and INT2 for bumpers
}
//this should be implemented when debugging is done.  shifts all interrupts onto status
//change pins
/*

	//whisker interrupt
	if(INTCON0bits.RBIF == 1){
		
		enum {
			off_wall = 0x00,
			tail_contact = 0x10,
			mid_contact = 0x20,
			front_off = 0x30,
			front_contact = 0x40,
			wedge = 0x50,
			tail_off = 0x60,
			all_good = 0x70,
		};
	
		switch(PORTB & 0x70) {
			case off_wall:
				servo_position = SERVO_LEFT_45;
				break;
			case tail_contact:
				servo_position = SERVO_LEFT_45;
				break;
			case mid_contact:
				servo_position = SERVO_LEFT_23;
				break;
			case front_off:
				servo_position = SERVO_LEFT_23;
				break;
			case front_contact:
				servo_position = SERVO_RIGHT_23;
				break;
			case wedge:
				servo_position = SERVO_STRAIGHT;  //try to force a different state
				break;
			case tail_off:
				servo_position = SERVO_RIGHT_23;  //may need refinement (2 cases: 
												  //(1)just finished a turn, straighten out
												  //(2)starting to drive nose into wall, right_23)
				break; 
			case all_good:
				servo_position = SERVO_STRAIGHT;
				break;
			default:
				servo_position = SERVO_STRAIGHT;
				break;
		}
		INTCON0bits.RBIF = 0;  //clear flag
	}
}
*/